// 通过闭式求解计算minimum acce/jerk/snap轨迹
#ifndef TRAJECTORY_GENERATOR_H
#define TRAJECTORY_GENERATOR_H

#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/QR>

namespace Planner
{
    using Eigen::MatrixXd;
    using Eigen::Quaterniond;
    using Eigen::Vector3d;
    using std::vector;

    struct State
    {
        Vector3d pos;
        Vector3d vel;
        Vector3d acc;
        double t;
        int seg;
    };

    namespace solve_mode
    {
        static constexpr int ACCE = 2;
        static constexpr int JERK = 3;
        static constexpr int SNAP = 4;
    } // namespace solve_mode

    class TrajectoryGenerator
    {
        // TODO 最小化snap需考虑jerk连续，故目前的实现只适合用于最小化jerk，acce.
        //      另外，最好将A构造成方阵，及pva则取5次多项式；
        //      还有一个问题是最小化jerk是否要考虑jerk连续？
        //      如果要的话那么最小化jerk对应7次，最小化snap对应9阶，就和mav_traj_gen的代码实现对的上了
    protected:
        int order_;                   // 轨迹阶数
        int continus_;                // 轨迹连续性
        int segments_;                // 轨迹段数
        int type_;                    // 求解类型
        double speed_;                // 预设速度
        vector<double> time_seq_;     // 分段轨迹时间戳
        vector<Vector3d> way_points_; // 路径点
        bool useDefaultTimeSeq;       // 是否主动设置时间戳
        // 边界条件
        Vector3d startVel_;
        Vector3d startAcc_;
        Vector3d goalVel_;
        Vector3d goalAcc_;

        MatrixXd coeffsx_, coeffsy_, coeffsz_;
        MatrixXd Q_;
        MatrixXd A_;
        MatrixXd MC_;
        MatrixXd AiMC_;
        MatrixXd dfx_, dfy_, dfz_;
        MatrixXd dpx_, dpy_, dpz_;
        MatrixXd R_, Rpp_, Rfp_;
        // MatrixXd df_, dp_;
        double cost_;

        // 时间分配
        void timeAllocation();

        // Q仅与时间有关
        void computeQ();
        void computeQi(MatrixXd &Qi, double time_seq);

        // 连乘函数
        double prod(int m, int n = 1);

        // 闭式法中Ap=d,对于派生类无需变动
        void computeA();

        // 构建选择矩阵,自由变量构建改变时需变动
        virtual void computeMC();

        // 计算固定变量,整合至df中
        virtual void computeFixVal(MatrixXd &df, int axis);

        // 分别对x\y\z轴闭式求解
        virtual void computeSingleAxis(const MatrixXd &df, MatrixXd &coeffs, MatrixXd &dp);

        // 计算自由变量、固定变量数量
        virtual void numFixFree(int &num_fix, int &num_free);

        // 任意矩阵求逆
        MatrixXd inv(const MatrixXd &matrix);
        // 计算矩阵的广义逆
        template <typename _Matrix_Type_>
        _Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon());

        // 将coeffs转化为segments_*(order_+1)
        MatrixXd resize(const MatrixXd &coeffs);

    public:
        TrajectoryGenerator(){};
        ~TrajectoryGenerator(){};

        // 设置求解参数
        void setParams(int order, double speed, int continus = 3);

        // 设置离散路径点
        void setWayPoints(const vector<Vector3d> &way_points);

        // 设置边界条件
        void setBoundaryCondition(const Vector3d &startVel, const Vector3d &startAcc,
                                  const Vector3d &goalVel, const Vector3d &goalAcc);

        // 闭式求解
        void solve(int type);

        // 人为设置时间戳，不使用默认方法计算
        void setTimeSeq(vector<double> &time_seq);

        void getTimeSeq(vector<double> &time_seq);

        void getCoeffs(vector<MatrixXd> &coeffs);

        // 外部设置多项式系数,优化过程会使用到
        void setCoeffs(vector<MatrixXd> &coeffs);

        // 得到用于优化的中间量
        void getVals(MatrixXd &R, MatrixXd &Rfp, MatrixXd &Rpp, vector<MatrixXd> &dp,
                     vector<MatrixXd> &df, MatrixXd &AiMC);

        // 平滑项代价函数
        double getCost();

        // 已知自由变量和时间戳，计算代价函数及轨迹多项式系数（用于优化步骤）
        void solveByDpAndTime(const vector<MatrixXd> &dfree, const vector<double> &time_seq,
                              vector<MatrixXd> &coeffs, double &cost);

        // 计算给定时刻的pos\vel\acc
        void getPos(double t, Vector3d &pos, int seg = 0, bool setSeg = false);

        void getVel(double t, Vector3d &vel, int seg = 0, bool setSeg = false);

        void getAcc(double t, Vector3d &acc, int seg = 0, bool setSeg = false);

        void getState(double t, State &curr_state, int seg = 0, bool setSeg = false);
    };

} // namespace Planner

#endif
